﻿import Hobot.GPIO as GPIO
import smbus2
import time
import math
import threading

# 清理可能已经被导出的通道
GPIO.cleanup()

# GPIO初始化
GPIO.setmode(GPIO.BOARD)

# 接口编号
LEFT_PWM_PIN = 33   # 左轮前向PWM接口
LEFT_GPIO_PIN = 37  # 左轮后向GPIO接口
RIGHT_PWM_PIN = 32  # 右轮前向PWM接口
RIGHT_GPIO_PIN = 36 # 右轮后向GPIO接口

# 初始化GPIO
GPIO.setup(LEFT_GPIO_PIN, GPIO.OUT)
GPIO.setup(RIGHT_GPIO_PIN, GPIO.OUT)

# 初始化PWM信号
left_pwm = GPIO.PWM(LEFT_PWM_PIN, 48000)  # PWM频率48kHz
right_pwm = GPIO.PWM(RIGHT_PWM_PIN, 48000)

# 设置初始状态
left_pwm.ChangeDutyCycle(1)
right_pwm.ChangeDutyCycle(1)
left_pwm.start(0)
right_pwm.start(0)

# PID参数
Kp = 1.0
Ki = 0.0
Kd = 0.0

# 初始化PID变量
prev_error = 0.0
integral = 0.0
dt = 0.05  # 采样时间50ms

# 初始Yaw角度为0
yaw_setpoint = 0.0
current_yaw = 0.0
pid_active = False  # 控制PID是否激活的标志位

# MPU6050传感器地址和寄存器定义
MPU6050_ADDR = 0x68
PWR_MGMT_1   = 0x6B
SMPLRT_DIV   = 0x19
CONFIG       = 0x1A
GYRO_CONFIG  = 0x1B
ACCEL_CONFIG = 0x1C
GYRO_XOUT_H  = 0x43
GYRO_YOUT_H  = 0x45
GYRO_ZOUT_H  = 0x47

# I2C总线初始化
bus = smbus2.SMBus(0)

def MPU6050_start():
    bus.write_byte_data(MPU6050_ADDR, SMPLRT_DIV, 0)
    time.sleep(0.1)
    bus.write_byte_data(MPU6050_ADDR, PWR_MGMT_1, 0x00)
    time.sleep(0.1)
    bus.write_byte_data(MPU6050_ADDR, PWR_MGMT_1, 0x01)
    time.sleep(0.1)
    bus.write_byte_data(MPU6050_ADDR, CONFIG, 0)
    time.sleep(0.1)
    bus.write_byte_data(MPU6050_ADDR, GYRO_CONFIG, 0)
    time.sleep(0.1)
    bus.write_byte_data(MPU6050_ADDR, ACCEL_CONFIG, 0)
    time.sleep(0.1)

def read_raw_bits(register):
    high = bus.read_byte_data(MPU6050_ADDR, register)
    low = bus.read_byte_data(MPU6050_ADDR, register+1)
    value = (high << 8) | low
    if value > 32768:
        value -= 65536
    return value

def read_gyroscope():
    gyro_z = read_raw_bits(GYRO_ZOUT_H)
    return (gyro_z / 131.0) * dt  # 将原始数据转换为角速度 (°/s)

MPU6050_start()

def pid_control():
    global prev_error, integral, current_yaw, pid_active
    base_speed = 80  # 设置基础速度
    while True:
        if pid_active:
            # 读取当前Yaw角度
            yaw_rate = read_gyroscope()
            current_yaw += yaw_rate

            # 计算误差
            error = yaw_setpoint - current_yaw

            # 计算PID输出
            integral += error * dt
            derivative = (error - prev_error) / dt
            output = Kp * error + Ki * integral + Kd * derivative
            prev_error = error

            # 根据PID输出调整轮速
            if output > 0:
                # 右偏，减慢右轮速度
                left_pwm.ChangeDutyCycle(min(100, base_speed + output))
                GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
                right_pwm.ChangeDutyCycle(max(70, base_speed - output))
                GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
            elif output < 0:
                # 左偏，减慢左轮速度
                left_pwm.ChangeDutyCycle(max(70, base_speed + output))
                GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
                right_pwm.ChangeDutyCycle(min(100, base_speed - output))
                GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
            else:
                # 直行
                left_pwm.ChangeDutyCycle(base_speed)
                GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
                right_pwm.ChangeDutyCycle(base_speed)
                GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)

        time.sleep(dt)

def keyboard_control():
    global pid_active
    import sys, tty, termios
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    tty.setcbreak(fd)

    try:
        while True:
            key = sys.stdin.read(1)
            if key == 'w':
                print("Forward")
                pid_active = True
            elif key == 's':
                print("Backward")
                pid_active = False
                # 向后
                left_pwm.ChangeDutyCycle(0)
                GPIO.output(LEFT_GPIO_PIN, GPIO.HIGH)
                right_pwm.ChangeDutyCycle(0)
                GPIO.output(RIGHT_GPIO_PIN, GPIO.HIGH)
            elif key == 'a':
                print("Left")
                pid_active = False
                # 左转
                left_pwm.ChangeDutyCycle(0)
                GPIO.output(LEFT_GPIO_PIN, GPIO.HIGH)
                right_pwm.ChangeDutyCycle(80)
                GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
            elif key == 'd':
                print("Right")
                pid_active = False
                # 右转
                left_pwm.ChangeDutyCycle(80)
                GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
                right_pwm.ChangeDutyCycle(0)
                GPIO.output(RIGHT_GPIO_PIN, GPIO.HIGH)
            # elif key == '':
            #     pid_active = False
            #     # 停止
            #     left_pwm.ChangeDutyCycle(0)
            #     GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
            #     right_pwm.ChangeDutyCycle(0)
            #     GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
            else:
                pid_active = False
                # 停止
                left_pwm.ChangeDutyCycle(0)
                GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
                right_pwm.ChangeDutyCycle(0)
                GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
                # pass
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)

# 开启键盘控制线程
keyboard_thread = threading.Thread(target=keyboard_control)
keyboard_thread.start()

# 开启PID控制线程
pid_thread = threading.Thread(target=pid_control)
pid_thread.start()

keyboard_thread.join()
pid_thread.join()
